#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/Imu.h>
#include <stdbool.h>
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int16.h>

#include "tail_control.h"

bool start_flag = false;
ros::Publisher motor_pub[3];
static void imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg)
{
    if((imu_msg->linear_acceleration.x > 0.1f) || (imu_msg->linear_acceleration.x < -0.1f) || \
        (imu_msg->linear_acceleration.y > 0.1f) || (imu_msg->linear_acceleration.y < -0.1f) || \
        (imu_msg->linear_acceleration.z > 0.1f) || (imu_msg->linear_acceleration.z < -0.1f)) {
        start_flag = true;
    } else {
        start_flag = false;
    }
}


static void flagCallback(const std_msgs::Int16::ConstPtr &flag_msg)
{
    if(flag_msg->data > 0) {
        start_flag = true;
    } else {
        start_flag = false;   
    }

}

std::shared_ptr<TailControl> control;

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "tail_control_test");
    ros::NodeHandle n;
    ros::AsyncSpinner spinner(3);
    std::string data_file;

    if(!n.getParam("data_file", data_file)) {
        ROS_ERROR("failed to retrieve datafile.");
        return -1;
    }
    ros::Subscriber imu_sub = n.subscribe<sensor_msgs::Imu>("/imu/data", 10, imuCallback);
    ros::Subscriber flag_sub = n.subscribe<std_msgs::Int16>("/flag", 10, flagCallback);
    motor_pub[0] = n.advertise<std_msgs::Float32MultiArray>("/motordata0", 10);
    motor_pub[1] = n.advertise<std_msgs::Float32MultiArray>("/motordata1", 10);
    motor_pub[2] = n.advertise<std_msgs::Float32MultiArray>("/motordata2", 10);
    spinner.start();

    try {
        control = std::make_shared<TailControl>(data_file, 100);
        control->tailInit();
        ros::waitForShutdown();    
    } catch (const ros::Exception& e) {
        ROS_FATAL_STREAM("Error in the hard interface:\n" << "\t" << e.what());
        return -1;
    }
    return 0;
}